package Tests
{
	import Box2D.Collision.Shapes.b2PolygonShape;
	import Box2D.Collision.b2TOIInput;
	import Box2D.Collision.b2TOIOutput;
	import Box2D.Collision.b2TimeOfImpact;
	import Box2D.Common.Math.b2Math;
	import Box2D.Common.Math.b2Sweep;
	import Box2D.Common.Math.b2Transform;
	import Box2D.Common.Math.b2Vec2;
	import Box2D.Common.b2Color;
	import Box2D.Common.b2Settings;
	
	import Framework.StringUtil;
	import Framework.Test;
	
	public class TimeOfImpact extends Test
	{
		private var m_shapeA:b2PolygonShape = new b2PolygonShape;
		private var m_shapeB:b2PolygonShape = new b2PolygonShape;
		
		public function TimeOfImpact()
		{
			super();
			m_shapeA.SetAsBox(25.0, 5.0);
			m_shapeB.SetAsBox(2.5, 2.5);
		}
		public override function Step():void{
			super.Step();
			
			var sweepA:b2Sweep = new b2Sweep;
			sweepA.c0.Set(24.0, -60.0);
			sweepA.a0 = 2.95;
			sweepA.c = sweepA.c0;
			sweepA.a = sweepA.a0;
			sweepA.localCenter.SetZero();
			
			var sweepB:b2Sweep = new b2Sweep;
			sweepB.c0.Set(53.474274, -50.252514);
			sweepB.a0 = 513.36676; // - 162.0f * b2_pi;
			sweepB.c.Set(54.595478, -51.083473);
			sweepB.a = 513.62781; //  - 162.0f * b2_pi;
			sweepB.localCenter.SetZero();
			
			//sweepB.a0 -= 300.0f * b2_pi;
			//sweepB.a -= 300.0f * b2_pi;
			
			var input:b2TOIInput =new b2TOIInput;
			input.proxyA.Set(m_shapeA, 0);
			input.proxyB.Set(m_shapeB, 0);
			input.sweepA = sweepA;
			input.sweepB = sweepB;
			input.tMax = 1.0;
			
			var output:b2TOIOutput = new b2TOIOutput;
			
			b2TimeOfImpact.TimeOfImpact(output,input);
			
			writeMessage("toi = " + output.t);
			
			var msg:String = StringUtil.sprintf("max toi iters = %d, max root iters = %d", b2Settings.b2_toiMaxIters, b2Settings.b2_toiMaxRootIters);
			writeMessage(msg);
			
			var vertices:Vector.<b2Vec2> = new Vector.<b2Vec2>(b2Settings.b2_maxPolygonVertices);
			
			var transformA:b2Transform = new b2Transform;
			sweepA.GetTransform(transformA, 0.0);
			for (var i:int = 0; i < m_shapeA.GetVertexCount(); ++i)
			{
				vertices[i] = b2Math.MulX(transformA, m_shapeA.vertices[i]);
			}
			var clr:b2Color = new b2Color;
			clr.Set(.9,.9,.9)
			m_debugDraw.DrawPolygon(vertices, m_shapeA.GetVertexCount(), clr);
			
			var transformB:b2Transform = new b2Transform;
			sweepB.GetTransform(transformB, 0.0);
			
			var localPoint:b2Vec2 = new b2Vec2(2.0, -0.1);
			var rB:b2Vec2 = b2Math.MulX(transformB, localPoint).Subtract(sweepB.c0);
			var wB:Number = sweepB.a - sweepB.a0;
			var vB:b2Vec2 = sweepB.c.Subtract(sweepB.c0);
			var v:b2Vec2 = vB.Add(b2Math.CrossFV(wB, rB));
			
			for (i = 0; i < m_shapeB.GetVertexCount(); ++i)
			{
				vertices[i] = b2Math.MulX(transformB, m_shapeB.vertices[i]);
			}
			clr.Set(0.5, 0.9, 0.5);
			m_debugDraw.DrawPolygon(vertices, m_shapeB.GetVertexCount(),clr);
			
			sweepB.GetTransform(transformB, output.t);
			for (i = 0; i < m_shapeB.GetVertexCount(); ++i)
			{
				vertices[i] =  b2Math.MulX(transformB, m_shapeB.vertices[i]);
			}
			clr.Set(0.5, 0.7, 0.9);
			m_debugDraw.DrawPolygon(vertices, m_shapeB.GetVertexCount(), clr);
			
			sweepB.GetTransform(transformB, 1.0);
			for (i = 0; i < m_shapeB.GetVertexCount(); ++i)
			{
				vertices[i] = b2Math.MulX(transformB, m_shapeB.vertices[i]);
			}
			clr.Set(0.9, 0.5, 0.5);
			m_debugDraw.DrawPolygon(vertices, m_shapeB.GetVertexCount(), clr);
		}
	}
}